#include <cstdio>
#include <iostream>
#include <cstring>

#include "SocketClient.h"
#include "protocol.h"

void help()
{
    std::cout << "move <x> <y> <z> <roll> <pitch> <yaw>" << std::endl;
    std::cout << "pollinputs" << std::endl;
    std::cout << "start" << std::endl;
    std::cout << "stop" << std::endl;
    std::cout << "home" << std::endl;
    std::cout << "park" << std::endl;
    std::cout << "follow" << std::endl;
    std::cout << "getstate" << std::endl;
    std::cout << "verbose" << std::endl;
}

int main(int argc, char* argv[])
{
    std::string addr;
    int port;
    if(argc > 1) addr = argv[1];
    else addr = "137.222.103.92";
    if(argc > 2) port = atoi(argv[2]);
    else port = 2006;
    
    SocketClient client(addr.c_str(), port);
    std::cout << "Enter some text, type \"help\" for help and \"quit\" to quit.\n";
    
    /*while (1)
    {
        cin.getline(buf, MAX_LINE, '\n');
        client.sendMsg(buf);
	    char *command, *parameters;
        command = strtok(buf, " ");
        parameters = strtok(NULL, "");
        if(strcmp(command, "move")==0){
        }
        else if(strcmp(command, "start") == 0) {
        }
        else if(strcmp(command, "stop") == 0) {
        }
        else if(strcmp(command, "home") == 0) {
        }
        else if(strcmp(command, "park") == 0) {
        }
        else if(strcmp(command, "follow") == 0) {
        }
        else if(strcmp(command, "getstate") == 0)
        {
            rec = client.receive();
            std::cout << rec << endl;
        }
        else if(strcmp(command, "pollinputs") == 0)
        {
            rec = client.receive();
            std::cout << rec << endl;
        }
        else if(!strcmp(command, "quit")) break;
	    else if(!strcmp(command, "help")) {
	        help();
	    }
	    memset(&buf,0,MAX_LINE+1);
    }*/
    
    // loop to test sending messages
    /*int i = 0;
    char msg[20];
    while(1)
    {
        sprintf(msg, "move %i 0 0 0 0 0", i);
        client.sendMsg(msg);
        i++;
    }*/
    
    // loop to test joystick polling
    polldata data;
    while(1)
    {
        printf("send command %i\n",cmdPOLLINPUTS);
        send_command(cmdPOLLINPUTS, (Socket*)&client);
        receive_polldata(&data, (Socket*)&client);
        for(int i=0; i<NUM_INPUTS; i++)
           fprintf(stdout, "%i\n", data->inputs[i]);
        
        /*movedata md;
        md.x = 0;
        md.y = 1;
        md.z = 2;
        md.r = 3;
        md.p = 4;
        md.w = 5;
        client.sendCommand(cmdMOVE);
        client.sendMoveCommand(&md);
        printf("move sent\n");*/
        Sleep(100);
    }
   
    client.disconnect();
    return 0;
}
